#include "UbtProtocol.h"


CUBTServoProtocol::CUBTServoProtocol()
{
}

CUBTServoProtocol::~CUBTServoProtocol()
{
}

uint8_t CUBTServoProtocol::Checksum(uint8_t *cmd)
{
	// Checmsum cmd[2, 7]

	uint16_t sum = 0;
	for (int i = 2;i <= 7; i++) {
		sum += cmd[i];
	}

	return (sum & 0xFF);
}

bool CUBTServoProtocol::PackSetAngle(int &size, uint8_t *cmd, uint8_t id, uint8_t angle,
	uint16_t msec, uint16_t lckmsec)
{
	// FA + AF + ID + 01 + Angle(0~240) + time(0~255,*20ms) + locktm(High) + locktm(Low) + CheckSum + ED
	// time & locktm: *20ms

	if (size < UBT_SERVO_CMD_SIZE)
		return false;
	
	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0x01;
	cmd[4] = angle;
	cmd[5] = (msec / 20);
	cmd[6] = ((lckmsec / 20) >> 8) & 0xFF;
	cmd[7] = (lckmsec / 20) & 0xFF;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;

	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackGetAngle(int &size, uint8_t *cmd, uint8_t id)
{
	// FA + AF + ID + 02 + 00 + 00 + 00 + 00 + CheckSum + ED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;
	
	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0x02;
	cmd[4] = 0x00;
	cmd[5] = 0x00;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;
	
	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackSetOffset(int &size, uint8_t *cmd, uint8_t id, int16_t offset)
{
	// FA + AF + ID + D2 + 00 + 00 + offset(High) + offset(Low) + CheckSum + ED
	// offset: 1/3 degree

	if (size < UBT_SERVO_CMD_SIZE)
		return false;
	
	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0xD2;
	cmd[4] = 0x00;
	cmd[5] = 0x00;
	cmd[6] = (offset >> 8) & 0xFF;
	cmd[7] = offset & 0xFF;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;

	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackGetOffset(int &size, uint8_t *cmd, uint8_t id)
{
	// FA + AF + ID + D4 + 00 + 00 + 00 + 00 + CheckSum + ED
	// offset: 1/3 degree
	
	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0xD4;
	cmd[4] = 0x00;
	cmd[5] = 0x00;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;
	
	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackSetLed(int &size, uint8_t *cmd, uint8_t id, bool on)
{
	// FA + AF + ID + 04 + on/off + 00 + 00 + 00 + CheckSum + ED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0x04;
	cmd[4] = on ? 0x00 : 0x01;
	cmd[5] = 0x00;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;

	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackSetId(int &size, uint8_t *cmd, uint8_t id, uint8_t newid)
{
	// FA + AF + ID + CD + 00 + NewID + 00 + 00 + CheckSum + ED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;
	
	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0xCD;
	cmd[4] = 0x00;
	cmd[5] = newid;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;

	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackTerminate(int &size, uint8_t *cmd, uint8_t id)
{
	// FA + AF + ID + 01 + FF + 00 + 00 + 00 + CheckSum + ED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	cmd[0] = 0xFA;
	cmd[1] = 0xAF;
	cmd[2] = id;
	cmd[3] = 0x01;
	cmd[4] = 0xFF;
	cmd[5] = 0x00;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;

	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::PackGetFirmware(int &size, uint8_t *cmd, uint8_t id)
{
	// FC + CF + ID + 01 + 00 + 00 + 00 + 00 + CheckSum + ED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;
	
	cmd[0] = 0xFC;
	cmd[1] = 0xCF;
	cmd[2] = id;
	cmd[3] = 0x01;
	cmd[4] = 0x00;
	cmd[5] = 0x00;
	cmd[6] = 0x00;
	cmd[7] = 0x00;
	cmd[8] = Checksum(cmd);
	cmd[9] = 0xED;
	
	size = UBT_SERVO_CMD_SIZE;
	return true;
}

bool CUBTServoProtocol::UnpackSetAngle(int size, uint8_t *cmd, uint8_t id)
{
	// (0xAA + id)

	if (size < 1)
		return false;

	if (cmd[0] != (0xAA + id))
		return false;

	return true;
}

bool CUBTServoProtocol::UnpackGetAngle(int size, uint8_t *cmd, uint8_t id, uint8_t *angle)
{
	// 0xFA 0xAF id 0xAA/0xEE ah al ah al 0xED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	uint8_t sum = Checksum(cmd);
	if (0xFA != cmd[0] || 0xAF != cmd[1] || id != cmd[2]
		|| 0xAA != cmd[3] || sum != cmd[8] || 0xED != cmd[9]) {
		return false;
	}
	//*angle = (cmd[6] << 8) | (cmd[7]);
	*angle = cmd[7];

	return true;
}

bool CUBTServoProtocol::UnpackSetOffset(int size, uint8_t *cmd, uint8_t id)
{
	// 0xFA 0xAF id 0xAA/0xEE 0x00 0x00 0x00 0x00 0xED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	uint8_t sum = Checksum(cmd);
	if (0xFA != cmd[0] || 0xAF != cmd[1] || id != cmd[2]
		|| 0xAA != cmd[3] || sum != cmd[8] || 0xED != cmd[9]) {
		return false;
	}

	return true;
}

bool CUBTServoProtocol::UnpackGetOffset(int size, uint8_t *cmd, uint8_t id, int16_t *offset)
{
	// 0xFA 0xAF id 0xAA/0xEE 0x00 0x00 offh offl 0xED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	uint8_t sum = Checksum(cmd);
	if (0xFA != cmd[0] || 0xAF != cmd[1] || id != cmd[2]
		|| 0xAA != cmd[3] || sum != cmd[8] || 0xED != cmd[9]) {
		return false;
	}
	*offset = (cmd[6] << 8) | (cmd[7]);

	return true;
}

bool CUBTServoProtocol::UnpackSetLed(int size, uint8_t *cmd, uint8_t id)
{
	// (0xAA + id)

	if (size < 1)
		return false;

	if (cmd[0] != (0xAA + id)) {
		return false;
	}

	return true;
}

bool CUBTServoProtocol::UnpackSetId(int size, uint8_t *cmd, uint8_t id)
{
	// 0xFA 0xAF newid 0xAA/0xEE 0x00 0x00 0x00 0x00 0xED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	uint8_t sum = Checksum(cmd);
	if (0xFA != cmd[0] || 0xAF != cmd[1] || id != cmd[2]
		|| 0xAA != cmd[3] || sum != cmd[8] || 0xED != cmd[9]) {
		return false;
	}

	return true;
}

bool CUBTServoProtocol::UnpackTerminate(int size, uint8_t *cmd, uint8_t id)
{
	// (0xAA + id)

	if (size < 1)
		return false;

	if (cmd[0] != (0xAA + id)) {
		return false;
	}

	return true;
}

bool CUBTServoProtocol::UnpackGetFirmware(int size, uint8_t *cmd, uint8_t id, uint32_t &firmware)
{
	// 0xFC 0xCF id 0xAA/0xEE v1 v2 v3 v4 0xED

	if (size < UBT_SERVO_CMD_SIZE)
		return false;

	uint8_t sum = Checksum(cmd);
	if (0xFC != cmd[0] || 0xCF != cmd[1] || (id != 0x00 && id != cmd[2])
		|| 0xAA != cmd[3] || sum != cmd[8] || 0xED != cmd[9]) {
		return false;
	}
	firmware = (cmd[4] << 24) | (cmd[5] << 16) | (cmd[6] << 8) | cmd[7];
	
	return true;
}

